//
//   Project Name:        KratosSolidMechanicsApplication $
//   Created by:          $Author:            JMCarbonell $
//   Last modified by:    $Co-Author:                     $
//   Date:                $Date:                July 2013 $
//   Revision:            $Revision:                  0.0 $
//
//

#if !defined(KRATOS_SOLID_MECHANICS_MATH_UTILITIES_H_INCLUDED)
#define KRATOS_SOLID_MECHANICS_MATH_UTILITIES_H_INCLUDED


#ifdef FIND_MAX
#undef FIND_MAX
#endif

#define FIND_MAX(a, b) ((a)>(b)?(a):(b))

// System includes
#include <cmath>

// External includes

// Project includes
#include "includes/global_variables.h"
#include "utilities/math_utils.h"
#include "geometries/point.h"


namespace Kratos
{
template<class TDataType>
class SolidMechanicsMathUtilities
{
public:
    /**
     * @name type definitions
     * @{
     */
    typedef Matrix MatrixType;

    typedef Vector VectorType;

    typedef unsigned int IndexType;

    typedef unsigned int SizeType;

    typedef MathUtils<TDataType> MathUtilsType;

    typedef DenseVector<Vector> Second_Order_Tensor;

    typedef DenseVector<Second_Order_Tensor> Third_Order_Tensor;

    typedef DenseVector<DenseVector<Matrix> > Fourth_Order_Tensor;

    typedef DenseMatrix<Second_Order_Tensor> Matrix_Second_Tensor;


    /**
     * @}
     */
    /**
     * calculates the solutions for a given cubic polynomial equation
     * 0= a*x^3+b*x^2+c*x+d
     * @param a coefficient
     * @param b coefficient
     * @param c coefficient
     * @param d coefficient
     * @param ZeroTol number treated as zero
     * @return Vector of solutions
     * WARNING only valid cubic (not quadratic, not linear, not constant) equations with
     * three real (not complex) solutions
     */

    static inline bool CardanoFormula(double a, double b, double c, double d, Vector& solution)
    {
        solution.resize(3,false);
        noalias(solution)= ZeroVector(3);

        if(a==0)
        {
            std::cout<<"This is not a cubic equation: CardanoFormula"<<std::endl;

            return false;
        }

        double p= (3.0*a*c-b*b)/(3.0*a*a);

        double q= 2.0*b*b*b/(27.0*a*a*a)-b*c/(3.0*a*a)+d/a;

        double discriminante= p*p*p/27.0+q*q/4.0;

        if(discriminante>0)
        {
            return false;
        }

        if(discriminante==0)
        {
            if( a == 0 )
                return false;

            solution(0)= pow(q/2.0, 1.0/3.0)-b/(3*a);
            solution(1)= pow(q/2.0, 1.0/3.0)-b/(3*a);
            solution(2)= pow(-4.0*q, 1.0/3.0)-b/(3*a);

            return true;
        }

        if(p < 0) return false; //in this case the square roots below will be negative. This substitutes with better efficiency lines 107-110

        solution(0)=
            -sqrt(-4.0/3.0*p)*cos(1.0/3.0*acos(-q/2.0*sqrt(-27.0/(p*p*p)))+ Globals::Pi / 3.0)
            -b/(3*a);
        solution(1)=
            sqrt(-4.0/3.0*p)*cos(1.0/3.0*acos(-q/2.0*sqrt(-27.0/(p*p*p))))-b/(3*a)
            ;
        solution(2)=
            -sqrt(-4.0/3.0*p)*cos(1.0/3.0*acos(-q/2.0*sqrt(-27.0/(p*p*p)))- Globals::Pi / 3.0)
            -b/(3*a);

//        if(std::isnan<double>(solution(0)) || std::isnan<double>(solution(1))|| std::isnan<double>(solution(2)))
//        {
//            return false;
//        }

        return true;
    }
    /**
     * @}
      */
    /**
     * calculates Eigenvalues of given square matrix A.
     * The QR Algorithm with shifts is used
     * @param A the given square matrix the eigenvalues are to be calculated.
     * @param tolerance convergence criteria
     * @param zero number treated as zero
     * @return Vector of eigenvalues
     * WARNING only valid for 2*2 and 3*3 Matrices yet
     */

    static inline Vector EigenValues(const Matrix& A, double tolerance, double zero)
    {
        int dim= A.size1();

        Matrix Convergence(2,dim);
	noalias(Convergence) = ZeroMatrix(2,dim);

        double delta = 0.0;

        double abs   = 0.0;

        Vector Result(dim);
	noalias(Result) = ZeroVector(dim);

        Matrix HelpA(dim,dim);
	noalias(HelpA) = ZeroMatrix(dim, dim);

        Matrix HelpQ(dim,dim);
	noalias(HelpQ) = ZeroMatrix(dim, dim);

        Matrix HelpR(dim,dim);
	noalias(HelpR) = ZeroMatrix(dim, dim);

        HelpA=A;

        bool is_converged=false;
        unsigned int max_iters = 10;
        unsigned int iter = 0;

        while(is_converged == false && iter<max_iters )
        {
            double shift= HelpA((dim-1),(dim-1));
            //
            for(int i=0; i<dim; i++)
            {
                HelpA(i,i) = HelpA(i,i)- shift;
            }

            SolidMechanicsMathUtilities<double>::QRFactorization(HelpA, HelpQ, HelpR);

            HelpA= ZeroMatrix(dim, dim);

            for(int i=0; i<dim; i++)
            {
                HelpA(i,i) += shift;
                for(int j=0; j< dim; j++)
                {
                    for(int k=0; k< dim; k++)
                    {
                        HelpA(i,j) += HelpR(i,k)*HelpQ(k,j);
                    }
                }
            }

            delta= 0.0;

            abs = 0.0;

            for(int i=0; i<dim; i++)
            {
                Convergence(0,i)=Convergence(1,i);
                Convergence(1,i)=HelpA(i,i);
                delta+= (Convergence(1,i)-Convergence(0,i))*(Convergence(1,i)-Convergence(0,i));
                abs+=(Convergence(1,i))*(Convergence(1,i));
            }

            delta= sqrt(delta);

            abs=sqrt(abs);

            if(abs < zero)
                abs=1.0;

            if(delta < zero || (delta/abs) < tolerance)
                is_converged=true;

            iter++;
        }


        for(int i=0; i<dim; i++)
        {
            Result(i)= HelpA(i,i);

            if(fabs(Result(i)) <zero)
                Result(i)=0.0;
        }

        return Result;
    }

    /**
     * @}
     */
    /**
     * calculates the eigenvectiors using a direct method.
     * @param A the given square matrix the eigenvalues are to be calculated.
     * WARNING only valid symmetric 3*3 Matrices
     */


    static inline Vector EigenValuesDirectMethod(const Matrix& A)
    {
        // Given a real symmetric 3x3 matrix A, compute the eigenvalues
        int dim= A.size1();
        Vector Result(dim);
	noalias(Result) = ZeroVector(dim);

        const double p1 = A(0,1)*A(0,1) + A(0,2)*A(0,2) + A(1,2)*A(1,2);
        if (p1 == 0)
        {//A is diagonal.
            Result[0] = A(0,0);
            Result[1] = A(1,1);
            Result[2] = A(2,2);
            return Result;
        }

        const double q = (A(0,0) + A(1,1) + A(2,2)) / 3.0;
        const double p2 = (A(0,0) - q) * (A(0,0) - q) + (A(1,1) - q) * (A(1,1) - q) + (A(2,2) - q) * (A(2,2) - q) + 2.0 * p1;
        const double p = sqrt(p2 / 6.0);

        Matrix B(3,3);
        const double inv_p = 1.0/p;

        // B = (1 / p) * (A - q * I)  where  I is the identity matrix
        B(0,0) = inv_p * (A(0,0) - q);
        B(1,1) = inv_p * (A(1,1) - q);
        B(2,2) = inv_p * (A(2,2) - q);
        B(0,1) = inv_p * A(0,1);
        B(1,0) = inv_p * A(1,0);
        B(0,2) = inv_p * A(0,2);
        B(2,0) = inv_p * A(2,0);
        B(1,2) = inv_p * A(1,2);
        B(2,1) = inv_p * A(2,1);

        //r = det(B) / 2
        double r = 0.5 * ( B(0,0)*B(1,1)*B(2,2) + B(0,1)*B(1,2)*B(2,0) + B(1,0)*B(2,1)*B(0,2) - B(2,0)*B(1,1)*B(0,2) - B(1,0)*B(0,1)*B(2,2) - B(0,0)*B(2,1)*B(1,2) );

        // In exact arithmetic for a symmetric matrix  -1 <= r <= 1
        // but computation error can leave it slightly outside this range.
        double phi = 0.0;
        if (r <= -1) { phi = Globals::Pi / 3.0; }
        else if (r >= 1) { phi = 0.0; }
        else { phi = acos(r) / 3.0;}

        // the eigenvalues satisfy eig3 <= eig2 <= eig1
        Result[0] = q + 2.0 * p * cos(phi);
        Result[2] = q + 2.0 * p * cos(phi + (2.0 * Globals::Pi / 3.0));
        Result[1] = 3.0 * q - Result[0] - Result[2];     //% since trace(A) = eig1 + eig2 + eig3

        return Result;
    }


    /**
     * @}
     */
    /**
     * calculates the QR Factorization of given square matrix A=QR.
     * The Factorization is performed using the householder algorithm
     * @param A the given square matrix the factorization is to be calculated.
     * @param Q the result matrix Q
     * @param R the result matrix R
     */

    static inline void QRFactorization(const MatrixType& A, MatrixType& Q, MatrixType& R)
    {

        //QR Factorization with Householder-Algo
        int dim= A.size1();

        Vector y(dim);

        Vector w(dim);

        R.resize(dim,dim,false);

        noalias(R)=ZeroMatrix(dim,dim);

        Q.resize(dim,dim,false);

        noalias(Q)=ZeroMatrix(dim,dim);

        Matrix Help(A.size1(),A.size2());
	noalias(Help) = A;

        Matrix unity(dim,dim);
	noalias(unity) = ZeroMatrix(dim,dim);

        for(int j=0; j<dim; j++)
            unity(j,j)=1.0;

        std::vector<Matrix> HelpQ(dim-1);

        std::vector<Matrix> HelpR(dim-1);

        for(int i=0; i< dim-1; i++)
        {
            HelpQ[i].resize(dim,dim,false);
            HelpR[i].resize(dim,dim,false);
            noalias(HelpQ[i])= unity;
            noalias(HelpR[i])= ZeroMatrix(dim,dim);
        }

        for(int iteration=0; iteration< dim-1; iteration++)
        {
            //Vector y
            for(int i=iteration; i<dim; i++)
                y(i)= Help(i,iteration);


            //Helpvalue l
            double normy=0.0;

            for(int i=iteration; i<dim; i++)
                normy += y(i)*y(i);

            normy= sqrt(normy);

            double l= sqrt((normy*(normy+fabs(y(iteration))))/2);

            double k=0.0;

            if(y[iteration] !=0)
                k= - y(iteration)/fabs(y(iteration))*normy;
            else
                k= -normy;

            for(int i=iteration; i<dim; i++)
            {
                double e=0;

                if(i==iteration)
                    e=1;

                w(i)= 1/(2*l)*(y(i)-k*e);
            }

            for(int i=iteration; i<dim; i++)
                for(int j=iteration; j<dim; j++)
                    HelpQ[iteration](i,j)= unity(i,j)- 2*w(i)*w(j);


            for(int i=iteration; i<dim; i++)
                for(int j=iteration; j<dim; j++)
                    for(int k=iteration; k<dim; k++)
                        HelpR[iteration](i,j)+= HelpQ[iteration](i,k)*Help(k,j);

            Help= HelpR[iteration];

        }

        //Assembling R
        for(int k=0; k<dim-1; k++)
        {
            for(int i=k; i<dim; i++)
                for(int j=k; j<dim; j++)
                    R(i,j) =HelpR[k](i,j);

        }


        for(int k=1; k<dim-1; k++)
        {
            for(int i=0; i<dim; i++)
                for(int j=0; j<dim; j++)
                    for(int l=0; l<dim; l++)
                        Q(i,j)+= HelpQ[(k-1)](i,l)*HelpQ[k](l,j);
            noalias(HelpQ[k])=Q;
        }
        if(dim-1==1)
            noalias(Q)=HelpQ[0];

    }

    /**
     * @}
     */
    /**
     * calculates the eigenvectors and eigenvalues of given symmetric matrix A.
     * The eigenvectors and eigenvalues are calculated using the iterative
     * Gauss-Seidel-method
     * @param A the given symmetric matrix where the eigenvectors have to be calculated.
     * @param vectors where the eigenvectors will be stored
     * @param lambda wher the eigenvalues will be stored
     * @param zero_tolerance the largest value considered to be zero
     * @param max_iterations allowed
     */


    static inline void EigenVectors(const MatrixType& A,
				    MatrixType& vectors,
				    VectorType& lambda,
				    double zero_tolerance =1e-9,
				    int max_iterations = 10)
    {
        Matrix Help= A;

        for(int i=0; i<3; i++)
            for(int j=0; j<3; j++)
                Help(i,j)= Help(i,j);


        vectors.resize(Help.size1(),Help.size2(),false);

        lambda.resize(Help.size1(),false);

        Matrix HelpDummy(Help.size1(),Help.size2());

        bool is_converged = false;

        Matrix unity(Help.size1(),Help.size2());
	noalias(unity) = ZeroMatrix(Help.size1(),Help.size2());

        for(unsigned int i=0; i< Help.size1(); i++)
            unity(i,i)= 1.0;

        Matrix V= unity;

        Matrix VDummy(Help.size1(),Help.size2());

        Matrix Rotation(Help.size1(),Help.size2());


        for(int iterations=0; iterations<max_iterations; iterations++)
        {

            is_converged= true;

            double a= 0.0;

            unsigned int index1= 0;

            unsigned int index2= 1;

            for(unsigned int i=0; i< Help.size1(); i++)
            {
                for(unsigned int j=(i+1); j< Help.size2(); j++)
                {
                    if((fabs(Help(i,j)) > a ) && (fabs(Help(i,j)) > zero_tolerance))
                    {
                        a= fabs(Help(i,j));

                        index1= i;
                        index2= j;

                        is_converged= false;
                    }
                }
            }

//                 KRATOS_WATCH( Help )

            if(is_converged)
                break;

            //Calculation of Rotationangle

            double gamma= (Help(index2,index2)-Help(index1,index1))/(2*Help(index1,index2));

            double u=1.0;

            if(fabs(gamma) > zero_tolerance && fabs(gamma)< (1/zero_tolerance))
            {
                u= gamma/fabs(gamma)*1.0/(fabs(gamma)+sqrt(1.0+gamma*gamma));
            }
            else
            {
                if  (fabs(gamma)>= (1.0/zero_tolerance))
                    u= 0.5/gamma;
            }

            double c= 1.0/(sqrt(1.0+u*u));

            double s= c*u;

            double teta= s/(1.0+c);

            //Ratotion of the Matrix
            HelpDummy= Help;

            HelpDummy(index2,index2)= Help(index2,index2)+u*Help(index1,index2);
            HelpDummy(index1,index1)= Help(index1,index1)-u*Help(index1,index2);
            HelpDummy(index1,index2)= 0.0;
            HelpDummy(index2,index1)= 0.0;

            for(unsigned int i=0; i<Help.size1(); i++)
            {
                if((i!= index1) && (i!= index2))
                {
                    HelpDummy(index2,i)=Help(index2,i)+s*(Help(index1,i)- teta*Help(index2,i));
                    HelpDummy(i,index2)=Help(index2,i)+s*(Help(index1,i)- teta*Help(index2,i));

                    HelpDummy(index1,i)=Help(index1,i)-s*(Help(index2,i)+ teta*Help(index1,i));
                    HelpDummy(i,index1)=Help(index1,i)-s*(Help(index2,i)+ teta*Help(index1,i));
                }
            }


            Help= HelpDummy;

            //Calculation of the eigenvectors V
            Rotation =unity;
            Rotation(index2,index1)=-s;
            Rotation(index1,index2)=s;
            Rotation(index1,index1)=c;
            Rotation(index2,index2)=c;

//                 Help.resize(A.size1(),A.size1(),false);
//                 noalias(Help)=ZeroMatrix(A.size1(),A.size1());

            noalias(VDummy) = ZeroMatrix(Help.size1(), Help.size2());

            for(unsigned int i=0; i< Help.size1(); i++)
            {
                for(unsigned int j=0; j< Help.size1(); j++)
                {
                    for(unsigned int k=0; k< Help.size1(); k++)
                    {
                        VDummy(i,j) += V(i,k)*Rotation(k,j);
                    }
                }
            }
            V= VDummy;

//                 Matrix VTA(3,3);
//                 noalias(VTA) = ZeroMatrix(3,3);
//                 for(int i=0; i< Help.size1(); i++)
//                 {
//                     for(int j=0; j< Help.size1(); j++)
//                     {
//                         for(int k=0; k< Help.size1(); k++)
//                         {
//                             VTA(i,j) += V(k,i)*A(k,j);
//                         }
//                     }
//                 }
//
//                 for(int i=0; i< Help.size1(); i++)
//                 {
//                     for(int j=0; j< Help.size1(); j++)
//                     {
//                         for(int k=0; k< Help.size1(); k++)
//                         {
//                             Help(i,j) += VTA(i,k)*V(k,j);
//                         }
//                     }
//                 }

        }

        if(!(is_converged))
        {
            std::cout<<"########################################################"<<std::endl;
            std::cout<<"Max_Iterations exceed in Jacobi-Seidel-Iteration (eigenvectors)"<<std::endl;
            std::cout<<"########################################################"<<std::endl;
        }

        for(unsigned int i=0; i< Help.size1(); i++)
        {
            for(unsigned int j=0; j< Help.size1(); j++)
            {
                vectors(i,j)= V(j,i);
            }
        }

        for(unsigned int i=0; i<Help.size1(); i++)
            lambda(i)= Help(i,i);

    }


    /**
     * @}
      */
    /**
     * calculates Eigenvectors and the EigenValues of given square matrix A(3x3)
     * The QR Algorithm with shifts is used
     * @param A the given symmetric 3x3 real matrix
     * @param V matrix to store the eigenvectors in rows
     * @param d matrix to store the eigenvalues
     */

    static inline void EigenVectors3x3(const Matrix& A, Matrix&V, Vector& d)
    {

      if(A.size1()!=3 || A.size2()!=3)
	std::cout<<" GIVEN MATRIX IS NOT 3x3  eigenvectors calculation "<<std::endl;

      Vector e(3);
      noalias(e) = ZeroVector(3);
      noalias(V) = ZeroMatrix(3,3);

      for (int i = 0; i < 3; i++) {
	for (int j = 0; j < 3; j++) {
	  V(i,j) = A(i,j);
	}
      }

      // *******************//
      //Symmetric Housholder reduction to tridiagonal form

      for (int j = 0; j < 3; j++) {
	d[j] = V(2,j);
      }

      // Householder reduction to tridiagonal form.

      for (int i = 2; i > 0; i--) {

	// Scale to avoid under/overflow.

	double scale = 0.0;
	double h = 0.0;
	for (int k = 0; k < i; k++) {
	  scale = scale + fabs(d[k]);
	}
	if (scale == 0.0) {
	  e[i] = d[i-1];
	  for (int j = 0; j < i; j++) {
	    d[j] = V(i-1,j);
	    V(i,j) = 0.0;
	    V(j,i) = 0.0;
	  }
	} else {

	  // Generate Householder vector.

	  for (int k = 0; k < i; k++) {
	    d[k] /= scale;
	    h += d[k] * d[k];
	  }
	  double f = d[i-1];
	  double g = sqrt(h);
	  if (f > 0) {
	    g = -g;
	  }
	  e[i] = scale * g;
	  h = h - f * g;
	  d[i-1] = f - g;
	  for (int j = 0; j < i; j++) {
	    e[j] = 0.0;
	  }

	  // Apply similarity transformation to remaining columns.

	  for (int j = 0; j < i; j++) {
	    f = d[j];
	    V(j,i) = f;
	    g = e[j] + V(j,j) * f;
	    for (int k = j+1; k <= i-1; k++) {
	      g += V(k,j) * d[k];
	      e[k] += V(k,j) * f;
	    }
	    e[j] = g;
	  }
	  f = 0.0;
	  for (int j = 0; j < i; j++) {
	    e[j] /= h;
	    f += e[j] * d[j];
	  }
	  double hh = f / (h + h);
	  for (int j = 0; j < i; j++) {
	    e[j] -= hh * d[j];
	  }
	  for (int j = 0; j < i; j++) {
	    f = d[j];
	    g = e[j];
	    for (int k = j; k <= i-1; k++) {
	      V(k,j) -= (f * e[k] + g * d[k]);
	    }
	    d[j] = V(i-1,j);
	    V(i,j) = 0.0;
	  }
	}
	d[i] = h;
      }

      // Accumulate transformations.

      for (int i = 0; i < 2; i++) {
	V(2,i) = V(i,i);
	V(i,i) = 1.0;
	double h = d[i+1];
	if (h != 0.0) {
	  for (int k = 0; k <= i; k++) {
	    d[k] = V(k,i+1) / h;
	  }
	  for (int j = 0; j <= i; j++) {
	    double g = 0.0;
	    for (int k = 0; k <= i; k++) {
	      g += V(k,i+1) * V(k,j);
	    }
	    for (int k = 0; k <= i; k++) {
	      V(k,j) -= g * d[k];
	    }
	  }
	}
	for (int k = 0; k <= i; k++) {
	  V(k,i+1) = 0.0;
	}
      }
      for (int j = 0; j < 3; j++) {
	d[j] = V(2,j);
	V(2,j) = 0.0;
      }
      V(2,2) = 1.0;
      e[0] = 0.0;

      // *******************//

      // Symmetric tridiagonal QL algorithm.

      for (int i = 1; i < 3; i++) {
	e[i-1] = e[i];
      }
      e[2] = 0.0;

      double f = 0.0;
      double tst1 = 0.0;
      double eps = std::pow(2.0,-52.0);
      for (int l = 0; l < 3; l++) {

	// Find small subdiagonal element

	tst1 = FIND_MAX(tst1,fabs(d[l]) + fabs(e[l]));
	int m = l;
	while (m < 3) {
	  if (fabs(e[m]) <= eps*tst1) {
	    break;
	  }
	  m++;
	}

	// If m == l, d[l] is an eigenvalue,
	// otherwise, iterate.

	if (m > l) {
	  int iter = 0;
	  do {
	    iter = iter + 1;  // (Could check iteration count here.)

	    // Compute implicit shift

	    double g = d[l];
	    double p = (d[l+1] - g) / (2.0 * e[l]);
	    double r = hypot2(p,1.0);
	    if (p < 0) {
	      r = -r;
	    }
	    d[l] = e[l] / (p + r);
	    d[l+1] = e[l] * (p + r);
	    double dl1 = d[l+1];
	    double h = g - d[l];
	    for (int i = l+2; i < 3; i++) {
	      d[i] -= h;
	    }
	    f = f + h;

	    // Implicit QL transformation.

	    p = d[m];
	    double c = 1.0;
	    double c2 = c;
	    double c3 = c;
	    double el1 = e[l+1];
	    double s = 0.0;
	    double s2 = 0.0;
	    for (int i = m-1; i >= l; i--) {
	      c3 = c2;
	      c2 = c;
	      s2 = s;
	      g = c * e[i];
	      h = c * p;
	      r = hypot2(p,e[i]);
	      e[i+1] = s * r;
	      s = e[i] / r;
	      c = p / r;
	      p = c * d[i] - s * g;
	      d[i+1] = h + s * (c * g + s * d[i]);

	      // Accumulate transformation.

	      for (int k = 0; k < 3; k++) {
		h = V(k,i+1);
		V(k,i+1) = s * V(k,i) + c * h;
		V(k,i) = c * V(k,i) - s * h;
	      }
	    }
	    p = -s * s2 * c3 * el1 * e[l] / dl1;
	    e[l] = s * p;
	    d[l] = c * p;

	    // Check for convergence.

	  } while (fabs(e[l]) > eps*tst1);
	}
	d[l] = d[l] + f;
	e[l] = 0.0;
      }

      // Sort eigenvalues and corresponding vectors.

      for (int i = 0; i < 2; i++) {
	int k = i;
	double p = d[i];
	for (int j = i+1; j < 3; j++) {
	  if (d[j] < p) {
	    k = j;
	    p = d[j];
	  }
	}
	if (k != i) {
	  d[k] = d[i];
	  d[i] = p;
	  for (int j = 0; j < 3; j++) {
	    p = V(j,i);
	    V(j,i) = V(j,k);
	    V(j,k) = p;
	  }
	}
      }

      V = trans(V);

      // *******************//


    }


    static double hypot2(double x, double y) {
      return std::sqrt(x*x+y*y);
    }

    /**
     * @}
     */
    /**
     * calculates the eigenvectors and eigenvalues of given matrix A.
     * The eigenvectors and eigenvalues are calculated using the iterative
     * JACOBI-method
     * @param A the given matrix the eigenvectors are to be calculated.
     * :WARNING: Matrix A will be overwritten
     * @param V the result matrix (will be overwritten with the eigenvectors)
     * @param error_tolerance the desired accuracy for the convergence check
     * @param zero_tolerance the largest value considered to be zero
     */
    static inline void EigenVectors( MatrixType& A,
                                     MatrixType& V,
                                     TDataType& error_tolerance,
                                     TDataType zero_tolerance)
    {
        //initial error
        TDataType error = 1.0;
        int n = A.size2();
        //setting V to identity matrix
	if( V.size1() != n )
	  V.resize(n,n,false);
	noalias(V) = IdentityMatrix(n);
        //calculation loop (as long as there is no convergence)
        //WARNING: iteration never exceeds
        while( error > error_tolerance )
        {
            for( int i=0; i<n; i++ )
            {
                for( int j=i+1; j<n; j++ )
                {
                    double theta = 0.0;
                    if( std::abs( A(i,j) ) >= zero_tolerance )
                    {
                        if( std::abs( A(i,i)-A(j,j) ) > 0.0 )
                        {
                            theta = 0.5*atan(2*A(i,j)/(A(i,i)-A(j,j)));
                        }
                        else theta = 0.25 * Globals::Pi;
                    }
                    MatrixType T(n,n);
		    noalias(T) = IdentityMatrix( n );

                    T(i,i) = cos(theta);
                    T(i,j) = -sin(theta);
                    T(j,i) = -T(i,j);
                    T(j,j) = T(i,i);

                    A = Mult( A, T );
                    MatrixType TT = Transpose(T);
                    A = Mult( TT, A );
                    V = Mult( V, T );
                }
            }
            double sTot = 0.0;
            double sDiag = 0.0;
            for( unsigned int i=0; i<A.size1(); i++ )
            {
                for( unsigned int j=0; j<A.size2(); j++ )
                {
                    sTot += std::abs(A(i,j));
                }
                sDiag+= std::abs(A(i,i));
            }
            error=(sTot-sDiag)/sDiag;
        }
        //sorting eigenvalues
        int maxIndex = 0;
        TDataType maxEv = A(0,0);
        for( unsigned int i=0; i<A.size1(); i++ )
        {
            for( unsigned int j=i; j<A.size1(); j++ )
            {
                //searching current maximum
                if( A(j,j) > maxEv )
                {
                    maxIndex = j;
                    maxEv = A(j,j);
                }
                //swapping eigenvalue matrix
                TDataType dummy = A(i,i);
                A(i,i) = A(maxIndex,maxIndex);
                A(maxIndex,maxIndex) = dummy;
                //swapping eigenvector matrix
                for( unsigned int k=0; k<A.size2(); k++ )
                {
                    dummy = V(k,i);
                    V(k,i) = V(k,maxIndex);
                    V(k,maxIndex) = dummy;
                }
            }

        }
    }//EigenVectors

    /**
     * creates identity matrix.
     * Given matrix will be overwritten
     * @param given matrix to be overwritten by identity matrix
     */
    static inline MatrixType IdentityMatrix( SizeType size )
    {
        MatrixType A(size,size);
        noalias(A) = ZeroMatrix( size, size );
        for( unsigned int i=0; i<size ; i++ )
        {
            A(i,i) = 1.0;
        }
        return A;
    }//IdentityMatrix

    /**
     * Adds two matrices. first argument is overwritten by sum of both
     * Matrices are assumed to be of same dimension (no check on boundaries is made!)
     * @param A first matrix argument (overwritten by solution)
     * @param B second matrix argument
     */
    static inline void Add( MatrixType& A, MatrixType& B )
    {
        for( unsigned int i=0; i<A.size1(); i++ )
        {
            for( unsigned int j=0; j<A.size2(); j++ )
            {
                A(i,j) += B(i,j);
            }
        }
    }

    /**
     * multiplies two matrices. Performs operation \f$ C = A B \f$
     * @param A matrix A
     * @param B matrix B
     * @return matrix \f$ C = A B \f$
     */
    static inline MatrixType Mult( MatrixType& A,
                                   MatrixType& B)
    {
        MatrixType C(A.size1(),B.size2());
        for( unsigned int i=0; i<A.size1(); i++ )
        {
            for( unsigned int j=0; j<B.size2(); j++ )
            {
                for( unsigned int k=0; k<A.size2(); k++ )
                {
                    C(i,j) += B(k,j)*A(i,k);
                }
            }
        }
        return C;
    }//Mult

    /**
     * multiplies a matrix by a scalar
     */
    static inline void Mult( MatrixType& M, TDataType a )
    {
        for( unsigned int i=0; i<M.size1(); i++ )
        {
            for( unsigned int j=0; j<M.size2(); j++ )
            {
                M(i,j) = M(i,j)*a;
            }
        }
    }

    /**
     * multiplies a vector by a scalar
     */
    static inline void Mult( VectorType& v, TDataType a )
    {
        for( unsigned int i=0; i<v.size(); i++ )
        {
            v(i) = v(i)*a;
        }
    }//Mult

    /**
     * transposes matrix A. Matrix A is not overwritten!
     * @param A the given Matrix
     * @return the transposed matrix \f$ A^T \f$
     */
    static inline MatrixType Transpose( MatrixType& A )
    {
        MatrixType AT(A.size2(),A.size1());
	noalias(AT) = ZeroMatrix(A.size2(),A.size1());
        for( unsigned int i=0; i<A.size1(); i++ )
        {
            for( unsigned int j=0; j<A.size2(); j++ )
            {
                AT(j,i) = A(i,j);
            }
        }
        return AT;
    }//Transpose

    /**
     * normalises a vector. Vector is scaled by \f$ V_{norm} = \frac{V}{|V|} \f$
     */
    static inline void Normalize( VectorType& v )
    {
        Mult( v, 1.0/(MathUtilsType::Norm( v )) );
    }

    /**
     * converts a strain vector into a matrix. Strains are assumed to be stored
     * in the following way:
     * \f$ [ e11, e22, e33, 2*e12, 2*e23, 2*e13 ] \f$ for 3D case and
     * \f$ [ e11, e22, 2*e12 ] \f$ fir 2D case.
     * Hence the deviatoric components of the strain vector are divided by 2
     * while they are stored into the matrix
     * @param Strains the given strain vector
     * @return the corresponding strain tensor in matrix form
     */
    static inline MatrixType StrainVectorToTensor( const VectorType& Strains )
    {
        KRATOS_TRY

        Matrix StrainTensor;
        //KRATOS_WATCH( Strains )
        if (Strains.size()==3)
        {
            StrainTensor.resize(2,2, false);
            //KRATOS_WATCH( StrainTensor )
            StrainTensor(0,0) = Strains[0];
            StrainTensor(0,1) = 0.5*Strains[2];
            StrainTensor(1,0) = 0.5*Strains[2];
            StrainTensor(1,1) = Strains[1];
        }
        else if (Strains.size()==6)
        {
            StrainTensor.resize(3,3, false);
            StrainTensor(0,0) = Strains[0];
            StrainTensor(0,1) = 0.5*Strains[3];
            StrainTensor(0,2) = 0.5*Strains[5];
            StrainTensor(1,0) = 0.5*Strains[3];
            StrainTensor(1,1) = Strains[1];
            StrainTensor(1,2) = 0.5*Strains[4];
            StrainTensor(2,0) = 0.5*Strains[5];
            StrainTensor(2,1) = 0.5*Strains[4];
            StrainTensor(2,2) = Strains[2];
        }

        //KRATOS_WATCH( StrainTensor )
        return StrainTensor;
        KRATOS_CATCH( "" )
    }

    static inline Vector TensorToStrainVector( const Matrix& Tensor )
    {
        KRATOS_TRY
        Vector StrainVector;


        if (Tensor.size1()==2)
        {
            StrainVector.resize(3);
            noalias(StrainVector) = ZeroVector(3);
            StrainVector(0) = Tensor(0,0);
            StrainVector(1) = Tensor(1,1);
            StrainVector(2) = 2.00*Tensor(0,1);
        }
        else if (Tensor.size1()==3)
        {
            StrainVector.resize(6);
            noalias(StrainVector) = ZeroVector(6);
            StrainVector(0) = Tensor(0,0);
            StrainVector(1) = Tensor(1,1);
            StrainVector(2) = Tensor(2,2);
            StrainVector(3) = 2.00*Tensor(0,1);
            StrainVector(4) = 2.00*Tensor(1,2);
            StrainVector(5) = 2.00*Tensor(0,2);
        }


        return StrainVector;
        KRATOS_CATCH( "" )
    }


    /**
    * Builds the Inverse of Matrix input
    * @param input the given Matrix
    * @param inverse inverse of the given Matrix
    */
    static int InvertMatrix( const MatrixType& input, MatrixType& inverse )
    {
      typedef permutation_matrix<std::size_t> pmatrix;
      Matrix A(input);
      pmatrix pm(A.size1());
      int singular = lu_factorize(A,pm);
      inverse.assign( IdentityMatrix(A.size1()));
      lu_substitute(A, pm, inverse);
      return singular;
    }

    /**
    * Builds the norm of a gibven second order tensor
    * @param Tensor the given second order tensor
    * @return the norm of the given tensor
    */
    static double NormTensor(Matrix& Tensor)
    {
        double result=0.0;
        for(unsigned int i=0; i< Tensor.size1(); i++)
            for(unsigned int j=0; j< Tensor.size2(); j++)
                result+= Tensor(i,j)*Tensor(i,j);

        result= sqrt(result);

        return result;
    }

    /**
    * Transforms a given 6*1 Vector to a corresponing symmetric Tensor of second order (3*3)
    * @param Vector the given vector
    * @param Tensor the symmetric second order tensor
    */
    static inline void VectorToTensor(const Vector& Stress,Matrix& Tensor)
    {
        if(Stress.size()==6)
        {
           Tensor.resize(3,3,false);
            Tensor(0,0)= Stress(0);
            Tensor(0,1)= Stress(3);
            Tensor(0,2)= Stress(5);
            Tensor(1,0)= Stress(3);
            Tensor(1,1)= Stress(1);
            Tensor(1,2)= Stress(4);
            Tensor(2,0)= Stress(5);
            Tensor(2,1)= Stress(4);
            Tensor(2,2)= Stress(2);
        }
        if(Stress.size()==3)
        {
            Tensor.resize(2,2,false);
            Tensor(0,0)= Stress(0);
            Tensor(0,1)= Stress(2);
            Tensor(1,0)= Stress(2);
            Tensor(1,1)= Stress(1);
        }

    }

    /**
    * Transforms a given symmetric Tensor of second order (3*3) to a corresponing 6*1 Vector
    * @param Tensor the given symmetric second order tensor
    * @param Vector the vector
    */
    static void TensorToVector( const Matrix& Tensor, Vector& Vector)
    {
        //if(Vector.size()!= 6)
        unsigned int  dim  =  Tensor.size1();
        if (dim==3)
        {
            Vector.resize(6,false);
            Vector(0)= Tensor(0,0);
            Vector(1)= Tensor(1,1);
            Vector(2)= Tensor(2,2);
            Vector(3)= Tensor(0,1);
            Vector(4)= Tensor(1,2);
            Vector(5)= Tensor(2,0);
        }
        else if(dim==2)
        {
            Vector.resize(3,false);
            Vector(0)= Tensor(0,0);
            Vector(1)= Tensor(1,1);
            Vector(2)= Tensor(0,1);
        }

    }

    static inline void TensorToMatrix(Fourth_Order_Tensor& Tensor,Matrix& Matrix)
    {


        // Simetrias seguras
        //  Cijkl = Cjilk;
        //  Cijkl = Cklji;
        if (Tensor[0].size()== 3)
        {
            // Tensor de cuarto orden cuyos componentes correspondes a una matriz de 3x3
            if(Matrix.size1()!=6 || Matrix.size2()!=6)
                Matrix.resize(6,6,false);
            Matrix(0,0) = Tensor[0][0](0,0);
            Matrix(0,1) = Tensor[0][0](1,1);
            Matrix(0,2) = Tensor[0][0](2,2);
            Matrix(0,3) = Tensor[0][0](0,1);
            Matrix(0,4) = Tensor[0][0](0,2);
            Matrix(0,5) = Tensor[0][0](1,2);

            Matrix(1,0) = Tensor[1][1](0,0);
            Matrix(1,1) = Tensor[1][1](1,1);
            Matrix(1,2) = Tensor[1][1](2,2);
            Matrix(1,3) = Tensor[1][1](0,1);
            Matrix(1,4) = Tensor[1][1](0,2);
            Matrix(1,5) = Tensor[1][1](1,2);

            Matrix(2,0) = Tensor[2][2](0,0);
            Matrix(2,1) = Tensor[2][2](1,1);
            Matrix(2,2) = Tensor[2][2](2,2);
            Matrix(2,3) = Tensor[2][2](0,1);
            Matrix(2,4) = Tensor[2][2](0,2);
            Matrix(2,5) = Tensor[2][2](1,2);

            Matrix(3,0) = Tensor[0][1](0,0);
            Matrix(3,1) = Tensor[0][1](1,1);
            Matrix(3,2) = Tensor[0][1](2,2);
            Matrix(3,3) = Tensor[0][1](0,1);
            Matrix(3,4) = Tensor[0][1](0,2);
            Matrix(3,5) = Tensor[0][1](1,2);

            Matrix(4,0) = Tensor[0][2](0,0);
            Matrix(4,1) = Tensor[0][2](1,1);
            Matrix(4,2) = Tensor[0][2](2,2);
            Matrix(4,3) = Tensor[0][2](0,1);
            Matrix(4,4) = Tensor[0][2](0,2);
            Matrix(4,5) = Tensor[0][2](1,2);

            Matrix(5,0) = Tensor[1][2](0,0);
            Matrix(5,1) = Tensor[1][2](1,1);
            Matrix(5,2) = Tensor[1][2](2,2);
            Matrix(5,3) = Tensor[1][2](0,1);
            Matrix(5,4) = Tensor[1][2](0,2);
            Matrix(5,5) = Tensor[1][2](1,2);
        }
        else
        {
            // Tensor de cuarto orden cuyos componentes correspondes a una matriz de 2x2
            if(Matrix.size1()!=3 || Matrix.size2()!=3)
                Matrix.resize(3,3,false);
            Matrix(0,0) = Tensor[0][0](0,0);
            Matrix(0,1) = Tensor[0][0](1,1);
            Matrix(0,2) = Tensor[0][0](0,1);
            Matrix(1,0) = Tensor[1][1](0,0);
            Matrix(1,1) = Tensor[1][1](1,1);
            Matrix(1,2) = Tensor[1][1](0,1);
            Matrix(2,0) = Tensor[0][1](0,0);
            Matrix(2,1) = Tensor[0][1](1,1);
            Matrix(2,2) = Tensor[0][1](0,1);

        }

    }

    /**
    * Transforms a given 6*6 Matrix to a corresponing 4th order tensor
    * @param Tensor the given Matrix
    * @param Vector the Tensor
    */
    static void MatrixToTensor(MatrixType& A,std::vector<std::vector<Matrix> >& Tensor)
    {
        int help1 = 0;
        int help2 = 0;
        double coeff = 1.0;

        Tensor.resize(3);

        for(unsigned int i=0; i<3; i++)
        {
            Tensor[i].resize(3);
            for(unsigned int j=0; j<3; j++)
            {
                Tensor[i][j].resize(3,3,false);
                noalias(Tensor[i][j])= ZeroMatrix(3,3);
                for(unsigned int k=0; k<3; k++)
                    for(unsigned int l=0; l<3; l++)
                    {
                        if(i==j) help1= i;
                        else
                        {
                            if((i==0 && j==1) || (i==1 && j==0)) help1= 3;
                            if((i==1 && j==2) || (i==2 && j==1)) help1= 4;
                            if((i==2 && j==0) || (i==0 && j==2)) help1= 5;
                        }
                        if(k==l)
                        {
                            help2= k;
                            coeff=1.0;
                        }
                        else
                        {
                            coeff=0.5;
                            if((k==0 && l==1) || (k==1 && l==0)) help2= 3;
                            if((k==1 && l==2) || (k==2 && l==1)) help2= 4;
                            if((k==2 && l==0) || (k==0 && l==2)) help2= 5;
                        }

                        Tensor[i][j](k,l)= A(help1,help2)*coeff;
                    }
            }
        }


    }
    /**
    * Transforms a given 6*6 Matrix to a corresponing 4th order tensor
    * @param Tensor the given Matrix
    * @param Vector the Tensor
    */
    static void MatrixToTensor(MatrixType& A,array_1d<double, 81>& Tensor)
    {
        int help1 = 0;
        int help2 = 0;
        double coeff = 1.0;
        std::fill(Tensor.begin(), Tensor.end(), 0.0);
        for(unsigned int i=0; i<3; i++)
        {
            for(unsigned int j=0; j<3; j++)
            {
                for(unsigned int k=0; k<3; k++)
                    for(unsigned int l=0; l<3; l++)
                    {
                        if(i==j) help1= i;
                        else
                        {
                            if((i==0 && j==1) || (i==1 && j==0)) help1= 3;
                            if((i==1 && j==2) || (i==2 && j==1)) help1= 4;
                            if((i==2 && j==0) || (i==0 && j==2)) help1= 5;
                        }
                        if(k==l)
                        {
                            help2= k;
                            coeff=1.0;
                        }
                        else
                        {
                            coeff=0.5;
                            if((k==0 && l==1) || (k==1 && l==0)) help2= 3;
                            if((k==1 && l==2) || (k==2 && l==1)) help2= 4;
                            if((k==2 && l==0) || (k==0 && l==2)) help2= 5;
                        }

                        Tensor[i*27+j*9+k*3+l]= A(help1,help2)*coeff;
                    }
            }
        }


    }
    /**
    * Transforms a given 4th order tensor to a corresponing 6*6 Matrix
    * @param Tensor the given Tensor
    * @param Vector the Matrix
    */
    static void TensorToMatrix(std::vector<std::vector<Matrix> >& Tensor,Matrix& Matrix)
    {
        int help1 = 0;
        int help2 = 0;
        int help3 = 0;
        int help4 = 0;
        double coeff = 1.0;

        if(Matrix.size1()!=6 || Matrix.size2()!=6)
            Matrix.resize(6,6,false);

        for(unsigned int i=0; i<6; i++)
            for(unsigned int j=0; j<6; j++)
            {
                if(i<3)
                {
                    help1= i;
                    help2= i;
                }
                else
                {
                    if(i==3)
                    {
                        help1= 0;
                        help2= 1;
                    }
                    if(i==4)
                    {
                        help1= 1;
                        help2= 2;
                    }
                    if(i==5)
                    {
                        help1= 2;
                        help2= 0;
                    }
                }

                if(j<3)
                {
                    help3= j;
                    help4= j;
                    coeff= 1.0;
                }
                else
                {
                    if(j==3)
                    {
                        help3= 0;
                        help4= 1;
                    }
                    if(j==4)
                    {
                        help3= 1;
                        help4= 2;
                    }
                    if(j==5)
                    {
                        help3= 2;
                        help4= 0;
                    }
                    coeff= 2.0;
                }

                Matrix(i,j)= Tensor[help1][help2](help3,help4)*coeff;
            }

    }

    /**
     * Transforms a given 4th order tensor to a corresponing 6*6 Matrix
     * @param Tensor the given Tensor
     * @param Vector the Matrix
     */
    static void TensorToMatrix( const array_1d<double, 81>& Tensor, Matrix& Matrix )
    {
        if(Matrix.size1()!=6 || Matrix.size2()!=6)
            Matrix.resize(6,6,false);

        Matrix(0,0) = Tensor[0];
        Matrix(0,1) = Tensor[4];
        Matrix(0,2) = Tensor[8];
        Matrix(0,3) = 2.0*Tensor[1];
        Matrix(0,4) = 2.0*Tensor[5];
        Matrix(0,5) = 2.0*Tensor[6];

        Matrix(1,0) = Tensor[36];
        Matrix(1,1) = Tensor[40];
        Matrix(1,2) = Tensor[44];
        Matrix(1,3) = 2.0*Tensor[37];
        Matrix(1,4) = 0.0*Tensor[41];
        Matrix(1,5) = 0.0*Tensor[42];

        Matrix(2,0) = Tensor[72];
        Matrix(2,1) = Tensor[76];
        Matrix(2,2) = Tensor[80];
        Matrix(2,3) = 2.0*Tensor[73];
        Matrix(2,4) = 2.0*Tensor[77];
        Matrix(2,5) = 2.0*Tensor[78];

        Matrix(3,0) = Tensor[9];
        Matrix(3,1) = Tensor[13];
        Matrix(3,2) = Tensor[18];
        Matrix(3,3) = 2.0*Tensor[10];
        Matrix(3,4) = 2.0*Tensor[14];
        Matrix(3,5) = 2.0*Tensor[15];

        Matrix(4,0) = Tensor[45];
        Matrix(4,1) = Tensor[49];
        Matrix(4,2) = Tensor[53];
        Matrix(4,3) = 2.0*Tensor[46];
        Matrix(4,4) = 0.0*Tensor[50];
        Matrix(4,5) = 0.0*Tensor[51];

        Matrix(5,0) = Tensor[54];
        Matrix(5,1) = Tensor[58];
        Matrix(5,2) = Tensor[62];
        Matrix(5,3) = 2.0*Tensor[55];
        Matrix(5,4) = 2.0*Tensor[59];
        Matrix(5,5) = 2.0*Tensor[60];

    }
    /**
    * Generates the fourth order deviatoric unity tensor
    * @param Unity the deviatoric unity (will be overwritten)
    */
    static void DeviatoricUnity(std::vector<std::vector<Matrix> >& Unity)
    {
        Unity.resize(3);

        Matrix kronecker(3,3);
        noalias(kronecker)=ZeroMatrix(3,3);
        for(unsigned int i=0; i<3; i++)
        {
            kronecker(i,i)=1;
        }

        for(unsigned int i=0; i<3; i++)
        {
            Unity[i].resize(3);
            for(unsigned int j=0; j<3; j++)
            {
                Unity[i][j].resize(3,3,false);
                noalias(Unity[i][j])= ZeroMatrix(3,3);

                for(unsigned int k=0; k<3; k++)
                {
                    for(unsigned int l=0; l<3; l++)
                    {
                        Unity[i][j](k,l)=kronecker(i,k)*kronecker(j,l)
                                         -1.0/3.0*kronecker(i,j)*kronecker(k,l);
                    }
                }
            }
        }
    }

    /**
     * Generates the fourth order deviatoric unity tensor
     * @param Unity the deviatoric unity (will be overwritten)
     */
    static void DeviatoricUnity(array_1d<double,81>& Unity)
    {
        Matrix kronecker(3,3);
        noalias(kronecker)=ZeroMatrix(3,3);
        for(unsigned int i=0; i<3; i++)
        {
            kronecker(i,i)=1;
        }

        for(unsigned int i=0; i<3; i++)
            for(unsigned int j=0; j<3; j++)
                for(unsigned int k=0; k<3; k++)
                    for(unsigned int l=0; l<3; l++)
                        Unity[27*i+9*j+3*k+l]=kronecker(i,k)*kronecker(j,l)
                                              -1.0/3.0*kronecker(i,j)*kronecker(k,l);
    }

    /**
    * Performs clipping on the two polygons clipping_points and subjected_points (the technique used is
    * Sutherland-Hodgman clipping) and returns the overlapping polygon result_points. The method works
    * in 3D. Both polygons have to be convex, but they can be slightly perturbated in 3D space, this
    * allows for performing clipping on two interpolated interfaces
    * @param clipping_points vertices of clipping polygon
    * @param subjected_points vertices of subjected polygon
    * @param result_points vertices of overlapping polygon
    * @return false= no overlapping polygon, true= overlapping polygon found
    */
    static bool Clipping(std::vector<Point* >& clipping_points,std::vector<Point* >& subjected_points, std::vector<Point* >& result_points, double tolerance)
    {
        result_points= subjected_points;
        Vector actual_edge(3);
        Vector actual_normal(3);
        std::vector<Point* > temp_results;
        bool is_visible= false;
        for(unsigned int clipp_edge=0; clipp_edge<clipping_points.size(); clipp_edge++)
        {
            temp_results.clear();
            unsigned int	index_clipp_2=0;
            if(clipp_edge< (clipping_points.size()-1))
                index_clipp_2= clipp_edge+1;
            //define clipping edge vector
            noalias(actual_edge)= *(clipping_points[clipp_edge])-*(clipping_points[index_clipp_2]);
            noalias(actual_edge)= actual_edge/sqrt(inner_prod(actual_edge,actual_edge));

            //define normal on clipping-edge vector towards visible side
            if(clipp_edge< (clipping_points.size()-2))
                actual_normal=*(clipping_points[clipp_edge+2])-(*(clipping_points[clipp_edge])+actual_edge*inner_prod((*(clipping_points[clipp_edge+2])-*(clipping_points[clipp_edge])),actual_edge));
            else if(clipp_edge< (clipping_points.size()-1))
                actual_normal=*(clipping_points[0])-(*(clipping_points[clipp_edge])+actual_edge*inner_prod((*(clipping_points[0])-*(clipping_points[clipp_edge])),actual_edge));
            else
                actual_normal=*(clipping_points[1])-(*(clipping_points[clipp_edge])+actual_edge*inner_prod((*(clipping_points[1])-*(clipping_points[clipp_edge])),actual_edge));

            noalias(actual_normal)=actual_normal/(sqrt(inner_prod(actual_normal,actual_normal)));

            //test if the first point is visible or unvisible
            if( inner_prod((*(result_points[0])-*(clipping_points[clipp_edge])), actual_normal)> tolerance)
            {
                is_visible= true;
            }
            else
            {
                is_visible= false;
            }

            for(unsigned int subj_edge=0; subj_edge< result_points.size(); subj_edge++)
            {
                unsigned int	index_subj_2=0;

                if(subj_edge< (result_points.size()-1))
                    index_subj_2= subj_edge+1;

                //Test whether the points of the actual subj_edge lay on clipp_edge
                if(fabs(inner_prod((*(result_points[subj_edge])-(*(clipping_points[clipp_edge]))), actual_normal))<= tolerance)
                {
                    temp_results.push_back(result_points[subj_edge]);
                    if( inner_prod((*(result_points[index_subj_2])-*(clipping_points[clipp_edge])), actual_normal)> tolerance)
                        is_visible= true;
                    else
                        is_visible= false;

                    continue;
                }
                //Calculate minimal distance between the two points
                Vector b(2);
                b(0)= -inner_prod((*(result_points[index_subj_2])-*(result_points[subj_edge])),(*(result_points[subj_edge])-*(clipping_points[clipp_edge])));
                b(1)= inner_prod((*(clipping_points[index_clipp_2])-*(clipping_points[clipp_edge])),(*(result_points[subj_edge])-*(clipping_points[clipp_edge])));
                Matrix A(2,2);
                A(0,0)=inner_prod((*(result_points[index_subj_2])-*(result_points[subj_edge])),(*(result_points[index_subj_2])-*(result_points[subj_edge])));
                A(0,1)=-inner_prod((*(result_points[index_subj_2])-*(result_points[subj_edge])),(*(clipping_points[index_clipp_2])-*(clipping_points[clipp_edge])));
                A(1,0)= A(0,1);
                A(1,1)=inner_prod(*(clipping_points[index_clipp_2])-*(clipping_points[clipp_edge]),*(clipping_points[index_clipp_2])-*(clipping_points[clipp_edge]));
                Vector coeff(2);
                coeff(0)=1.0/A(0,0)*(b(0)-A(0,1)/(A(1,1)-A(0,1)*A(1,0)/A(0,0))*(b(1)-b(0)*A(1,0)/A(0,0)));
                coeff(1)=1.0/(A(1,1)-A(0,1)*A(1,0)/A(0,0))*(b(1)-b(0)*A(1,0)/A(0,0));


                //TEST on distance to endpoints of the line
                Vector dist_vec(3);
                noalias(dist_vec)= *(result_points[subj_edge])+coeff(0)*(*(result_points[index_subj_2])-*(result_points[subj_edge]))-(*(clipping_points[clipp_edge])+coeff(1)*(*(clipping_points[index_clipp_2])-*(clipping_points[clipp_edge])));

                if( coeff(0) > tolerance && coeff(0) < (1-tolerance)&& (sqrt(inner_prod(dist_vec,dist_vec))< tolerance))
                {
                    if(is_visible)
                        temp_results.push_back(result_points[subj_edge]);

                    temp_results.push_back(new Point((*(clipping_points[clipp_edge])+coeff(1)*(*(clipping_points[index_clipp_2])-*(clipping_points[clipp_edge])))));

                    is_visible= !is_visible;

                    continue;
                }
                if(is_visible)
                    temp_results.push_back(result_points[subj_edge]);
            }
            result_points=temp_results;
        }
        if(result_points.size()==0)
            return false;
        else
            return true;
    }


  inline static VectorType range(const unsigned int start,  const unsigned int end)
  {
    int size = end-start;
    if(size<0)
      KRATOS_ERROR<<" range out of bounds start:"<<start<<" end:"<<end<<std::endl;

    VectorType b(size);

    for(int i = 0; i<size; ++i)
      b[i] = start + i;

    return b;
  }


  inline static MatrixType project(const MatrixType& rMatrix, VectorType irange, VectorType jrange)
  {

    MatrixType A(irange.size(), jrange.size());

    for(unsigned int i = 0; i<irange.size(); ++i)
      for(unsigned int j = 0; j<jrange.size(); ++j)
        A(i,j) = rMatrix(irange[i],jrange[j]);

    return A;
  }


  inline static MatrixType& project(MatrixType& rMatrixA, VectorType irange, VectorType jrange, const MatrixType& rMatrixB)
  {

    for(unsigned int i = 0; i<irange.size(); ++i)
      for(unsigned int j = 0; j<jrange.size(); ++j)
        rMatrixA(irange[i],jrange[j]) = rMatrixB(i,j);

    return rMatrixA;
  }


  inline static VectorType project(const VectorType& rVector, VectorType irange)
  {

    VectorType b(irange.size());

    for(unsigned int i = 0; i<irange.size(); ++i)
      b[i] = rVector[irange[i]];

    return b;
  }



private:
};// class SolidMechanicsMathUtilities
}
#endif /* KRATOS_SOLID_MECHANICS_MATH_UTILITIESS_H_INCLUDED defined */
